// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net) and Silicon Studio Corp. (https://www.siliconstudio.co.jp)
// Distributed under the MIT license. See the LICENSE.md file in the project root for more information.
//
// -----------------------------------------------------------------------------
// Original code from SlimMath project. http://code.google.com/p/slimmath/
// Greetings to SlimDX Group. Original code published with the following license:
// -----------------------------------------------------------------------------
/*
* Copyright (c) 2007-2011 SlimDX Group
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/

namespace Stride.Core.Mathematics;

/*
 * This class is organized so that the least complex objects come first so that the least
 * complex objects will have the most methods in most cases. Note that not all shapes exist
 * at this time and not all shapes have a corresponding struct. Only the objects that have
 * a corresponding struct should come first in naming and in parameter order. The order of
 * complexity is as follows:
 * 
 * 1. Point
 * 2. Ray
 * 3. Segment
 * 4. Plane
 * 5. Triangle
 * 6. Polygon
 * 7. Box
 * 8. Sphere
 * 9. Ellipsoid
 * 10. Cylinder
 * 11. Cone
 * 12. Capsule
 * 13. Torus
 * 14. Polyhedron
 * 15. Frustum
*/

/// <summary>
/// Contains static methods to help in determining intersections, containment, etc.
/// </summary>
public static class CollisionHelper
{
    /// <summary>
    /// Determines the closest point between a point and a triangle.
    /// </summary>
    /// <param name="point">The point to test.</param>
    /// <param name="vertex1">The first vertex to test.</param>
    /// <param name="vertex2">The second vertex to test.</param>
    /// <param name="vertex3">The third vertex to test.</param>
    /// <param name="result">When the method completes, contains the closest point between the two objects.</param>
    public static void ClosestPointPointTriangle(ref readonly Vector3 point, ref readonly Vector3 vertex1, ref readonly Vector3 vertex2, ref readonly Vector3 vertex3, out Vector3 result)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 136

        //Check if P in vertex region outside A
        Vector3 ab = vertex2 - vertex1;
        Vector3 ac = vertex3 - vertex1;
        Vector3 ap = point - vertex1;

        float d1 = Vector3.Dot(ab, ap);
        float d2 = Vector3.Dot(ac, ap);
        if (d1 <= 0.0f && d2 <= 0.0f)
        {
            result = vertex1; //Barycentric coordinates (1,0,0)
            return;
        }

        //Check if P in vertex region outside B
        Vector3 bp = point - vertex2;
        float d3 = Vector3.Dot(ab, bp);
        float d4 = Vector3.Dot(ac, bp);
        if (d3 >= 0.0f && d4 <= d3)
        {
            result = vertex2; // barycentric coordinates (0,1,0)
            return;
        }

        //Check if P in edge region of AB, if so return projection of P onto AB
        float vc = (d1 * d4) - (d3 * d2);
        if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
        {
            float v = d1 / (d1 - d3);
            result = vertex1 + (v * ab); //Barycentric coordinates (1-v,v,0)
            return;
        }

        //Check if P in vertex region outside C
        Vector3 cp = point - vertex3;
        float d5 = Vector3.Dot(ab, cp);
        float d6 = Vector3.Dot(ac, cp);
        if (d6 >= 0.0f && d5 <= d6)
        {
            result = vertex3; //Barycentric coordinates (0,0,1)
            return;
        }

        //Check if P in edge region of AC, if so return projection of P onto AC
        float vb = (d5 * d2) - (d1 * d6);
        if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
        {
            float w = d2 / (d2 - d6);
            result = vertex1 + (w * ac); //Barycentric coordinates (1-w,0,w)
            return;
        }

        //Check if P in edge region of BC, if so return projection of P onto BC
        float va = (d3 * d6) - (d5 * d4);
        if (va <= 0.0f && d4 >= d3 && d5 >= d6)
        {
            float w = (d4 - d3) / (d4 - d3 + (d5 - d6));
            result = vertex2 + (w * (vertex3 - vertex2)); //Barycentric coordinates (0,1-w,w)
            return;
        }

        //P inside face region. Compute Q through its barycentric coordinates (u,v,w)
        float denom = 1.0f / (va + vb + vc);
        float v2 = vb * denom;
        float w2 = vc * denom;
        result = vertex1 + (ab * v2) + (ac * w2); //= u*vertex1 + v*vertex2 + w*vertex3, u = va * denom = 1.0f - v - w
    }

    /// <summary>
    /// Determines the closest point between a <see cref="Plane"/> and a point.
    /// </summary>
    /// <param name="plane">The plane to test.</param>
    /// <param name="point">The point to test.</param>
    /// <param name="result">When the method completes, contains the closest point between the two objects.</param>
    public static void ClosestPointPlanePoint(ref readonly Plane plane, ref readonly Vector3 point, out Vector3 result)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 126

        Vector3.Dot(in plane.Normal, in point, out var dot);
        float t = dot - plane.D;

        result = point - (t * plane.Normal);
    }

    /// <summary>
    /// Determines the closest point between a <see cref="BoundingBox"/> and a point.
    /// </summary>
    /// <param name="box">The box to test.</param>
    /// <param name="point">The point to test.</param>
    /// <param name="result">When the method completes, contains the closest point between the two objects.</param>
    public static void ClosestPointBoxPoint(ref readonly BoundingBox box, ref readonly Vector3 point, out Vector3 result)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 130

        Vector3.Max(in point, in box.Minimum, out var temp);
        Vector3.Min(ref temp, in box.Maximum, out result);
    }

    /// <summary>
    /// Determines the closest point between a <see cref="BoundingSphere"/> and a point.
    /// </summary>
    /// <param name="sphere">The bounding sphere.</param>
    /// <param name="point">The point to test.</param>
    /// <param name="result">When the method completes, contains the closest point between the two objects;
    /// or, if the point is directly in the center of the sphere, contains <see cref="Vector3.Zero"/>.</param>
    public static void ClosestPointSpherePoint(ref readonly BoundingSphere sphere, ref readonly Vector3 point, out Vector3 result)
    {
        //Source: Jorgy343
        //Reference: None

        //Get the unit direction from the sphere's center to the point.
        Vector3.Subtract(in point, in sphere.Center, out result);
        result.Normalize();

        //Multiply the unit direction by the sphere's radius to get a vector
        //the length of the sphere.
        result *= sphere.Radius;

        //Add the sphere's center to the direction to get a point on the sphere.
        result += sphere.Center;
    }

    /// <summary>
    /// Determines the closest point between a <see cref="BoundingSphere"/> and a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="sphere1">The first sphere to test.</param>
    /// <param name="sphere2">The second sphere to test.</param>
    /// <param name="result">When the method completes, contains the closest point between the two objects;
    /// or, if the point is directly in the center of the sphere, contains <see cref="Vector3.Zero"/>.</param>
    /// <remarks>
    /// If the two spheres are overlapping, but not directly ontop of each other, the closest point
    /// is the 'closest' point of intersection. This can also be considered is the deepest point of
    /// intersection.
    /// </remarks>
    public static void ClosestPointSphereSphere(ref readonly BoundingSphere sphere1, ref readonly BoundingSphere sphere2, out Vector3 result)
    {
        //Source: Jorgy343
        //Reference: None

        //Get the unit direction from the first sphere's center to the second sphere's center.
        Vector3.Subtract(in sphere2.Center, in sphere1.Center, out result);
        result.Normalize();

        //Multiply the unit direction by the first sphere's radius to get a vector
        //the length of the first sphere.
        result *= sphere1.Radius;

        //Add the first sphere's center to the direction to get a point on the first sphere.
        result += sphere1.Center;
    }

    /// <summary>
    /// Determines the distance between a <see cref="Plane"/> and a point.
    /// </summary>
    /// <param name="plane">The plane to test.</param>
    /// <param name="point">The point to test.</param>
    /// <returns>The distance between the two objects.</returns>
    public static float DistancePlanePoint(ref readonly Plane plane, ref readonly Vector3 point)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 127

        Vector3.Dot(in plane.Normal, in point, out var dot);
        return dot - plane.D;
    }

    /// <summary>
    /// Determines the distance between a <see cref="BoundingBox"/> and a point.
    /// </summary>
    /// <param name="box">The box to test.</param>
    /// <param name="point">The point to test.</param>
    /// <returns>The distance between the two objects.</returns>
    public static float DistanceBoxPoint(ref readonly BoundingBox box, ref readonly Vector3 point)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 131

        float distance = 0f;

        if (point.X < box.Minimum.X)
            distance += (box.Minimum.X - point.X) * (box.Minimum.X - point.X);
        if (point.X > box.Maximum.X)
            distance += (point.X - box.Maximum.X) * (point.X - box.Maximum.X);

        if (point.Y < box.Minimum.Y)
            distance += (box.Minimum.Y - point.Y) * (box.Minimum.Y - point.Y);
        if (point.Y > box.Maximum.Y)
            distance += (point.Y - box.Maximum.Y) * (point.Y - box.Maximum.Y);

        if (point.Z < box.Minimum.Z)
            distance += (box.Minimum.Z - point.Z) * (box.Minimum.Z - point.Z);
        if (point.Z > box.Maximum.Z)
            distance += (point.Z - box.Maximum.Z) * (point.Z - box.Maximum.Z);

        return MathF.Sqrt(distance);
    }

    /// <summary>
    /// Determines the distance between a <see cref="BoundingBox"/> and a <see cref="BoundingBox"/>.
    /// </summary>
    /// <param name="box1">The first box to test.</param>
    /// <param name="box2">The second box to test.</param>
    /// <returns>The distance between the two objects.</returns>
    public static float DistanceBoxBox(ref readonly BoundingBox box1, ref readonly BoundingBox box2)
    {
        //Source:
        //Reference:

        float distance = 0f;

        //Distance for X.
        if (box1.Minimum.X > box2.Maximum.X)
        {
            float delta = box2.Maximum.X - box1.Minimum.X;
            distance += delta * delta;
        }
        else if (box2.Minimum.X > box1.Maximum.X)
        {
            float delta = box1.Maximum.X - box2.Minimum.X;
            distance += delta * delta;
        }

        //Distance for Y.
        if (box1.Minimum.Y > box2.Maximum.Y)
        {
            float delta = box2.Maximum.Y - box1.Minimum.Y;
            distance += delta * delta;
        }
        else if (box2.Minimum.Y > box1.Maximum.Y)
        {
            float delta = box1.Maximum.Y - box2.Minimum.Y;
            distance += delta * delta;
        }

        //Distance for Z.
        if (box1.Minimum.Z > box2.Maximum.Z)
        {
            float delta = box2.Maximum.Z - box1.Minimum.Z;
            distance += delta * delta;
        }
        else if (box2.Minimum.Z > box1.Maximum.Z)
        {
            float delta = box1.Maximum.Z - box2.Minimum.Z;
            distance += delta * delta;
        }

        return MathF.Sqrt(distance);
    }

    /// <summary>
    /// Determines the distance between a <see cref="BoundingSphere"/> and a point.
    /// </summary>
    /// <param name="sphere">The sphere to test.</param>
    /// <param name="point">The point to test.</param>
    /// <returns>The distance between the two objects.</returns>
    public static float DistanceSpherePoint(ref readonly BoundingSphere sphere, ref readonly Vector3 point)
    {
        //Source: Jorgy343
        //Reference: None

        Vector3.Distance(in sphere.Center, in point, out var distance);
        distance -= sphere.Radius;

        return MathF.Max(distance, 0f);
    }

    /// <summary>
    /// Determines the distance between a <see cref="BoundingSphere"/> and a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="sphere1">The first sphere to test.</param>
    /// <param name="sphere2">The second sphere to test.</param>
    /// <returns>The distance between the two objects.</returns>
    public static float DistanceSphereSphere(ref readonly BoundingSphere sphere1, ref readonly BoundingSphere sphere2)
    {
        //Source: Jorgy343
        //Reference: None

        Vector3.Distance(in sphere1.Center, in sphere2.Center, out var distance);
        distance -= sphere1.Radius + sphere2.Radius;

        return MathF.Max(distance, 0f);
    }

    /// <summary>
    /// Determines whether there is an intersection between a Line and a <see cref="Plane"/>.
    /// </summary>
    /// <param name="plane">The plane to test.</param>
    /// <param name="point1">The first point of the line to test.</param>
    /// <param name="point2">The second point of the line to test.</param> 
    /// <param name="intersection">When the method completes, contains the point of intersection,
    /// or <see cref="Vector3.Zero"/> if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool LinePlaneIntersection(Plane plane, Vector3 point1, Vector3 point2, out Vector3 intersection)
    {
        //Source: Mathematics for 3D Game Programming and Computer Graphics (3rd ed) by Eric Lengyel
        //Reference: Page 93, 99

        Vector3 normal = plane.Normal;
        Vector3 directionVector = point2 - point1;

        float denominator = Vector3.Dot(normal, directionVector);

        //Line is parallel to plane
        if (MathF.Abs(denominator) < MathUtil.ZeroTolerance)
        {
            intersection = Vector3.Zero;
            return false;
        }

        float t = -(Vector3.Dot(normal, point1) - plane.D) / denominator;

        intersection = point1 + (t * (directionVector));
        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a point.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="point">The point to test.</param>
    /// <returns>Whether the two objects intersect.</returns>
    public static bool RayIntersectsPoint(ref readonly Ray ray, ref readonly Vector3 point)
    {
        //Source: RayIntersectsSphere
        //Reference: None

        Vector3.Subtract(in ray.Position, in point, out var m);

        //Same thing as RayIntersectsSphere except that the radius of the sphere (point)
        //is the epsilon for zero.
        float b = Vector3.Dot(m, ray.Direction);
        float c = Vector3.Dot(m, m) - MathUtil.ZeroTolerance;

        if (c > 0f && b > 0f)
            return false;

        float discriminant = (b * b) - c;

        return discriminant >= 0f;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a <see cref="Ray"/>.
    /// </summary>
    /// <param name="ray1">The first ray to test.</param>
    /// <param name="ray2">The second ray to test.</param>
    /// <param name="point">When the method completes, contains the point of intersection,
    /// or <see cref="Vector3.Zero"/> if there was no intersection.</param>
    /// <returns>Whether the two objects intersect.</returns>
    /// <remarks>
    /// This method performs a ray vs ray intersection test based on the following formula
    /// from Goldman.
    /// <c>s = det([o_2 - o_1, d_2, d_1 x d_2]) / ||d_1 x d_2||^2</c>
    /// <c>t = det([o_2 - o_1, d_1, d_1 x d_2]) / ||d_1 x d_2||^2</c>
    /// Where o_1 is the position of the first ray, o_2 is the position of the second ray,
    /// d_1 is the normalized direction of the first ray, d_2 is the normalized direction
    /// of the second ray, det denotes the determinant of a matrix, x denotes the cross
    /// product, [ ] denotes a matrix, and || || denotes the length or magnitude of a vector.
    /// </remarks>
    public static bool RayIntersectsRay(ref readonly Ray ray1, ref readonly Ray ray2, out Vector3 point)
    {
        //Source: Real-Time Rendering, Third Edition
        //Reference: Page 780

        Vector3.Cross(in ray1.Direction, in ray2.Direction, out var cross);
        float denominator = cross.Length();

        //Lines are parallel.
        if (MathF.Abs(denominator) < MathUtil.ZeroTolerance)
        {
            //Lines are parallel and on top of each other.
            if (MathF.Abs(ray2.Position.X - ray1.Position.X) < MathUtil.ZeroTolerance &&
                MathF.Abs(ray2.Position.Y - ray1.Position.Y) < MathUtil.ZeroTolerance &&
                MathF.Abs(ray2.Position.Z - ray1.Position.Z) < MathUtil.ZeroTolerance)
            {
                point = Vector3.Zero;
                return true;
            }
        }

        denominator *= denominator;

        //3x3 matrix for the first ray.
        float m11 = ray2.Position.X - ray1.Position.X;
        float m12 = ray2.Position.Y - ray1.Position.Y;
        float m13 = ray2.Position.Z - ray1.Position.Z;
        float m21 = ray2.Direction.X;
        float m22 = ray2.Direction.Y;
        float m23 = ray2.Direction.Z;
        float m31 = cross.X;
        float m32 = cross.Y;
        float m33 = cross.Z;

        //Determinant of first matrix.
        float dets =
            (m11 * m22 * m33) +
            (m12 * m23 * m31) +
            (m13 * m21 * m32) -
            (m11 * m23 * m32) -
            (m12 * m21 * m33) -
            (m13 * m22 * m31);

        //3x3 matrix for the second ray.
        m21 = ray1.Direction.X;
        m22 = ray1.Direction.Y;
        m23 = ray1.Direction.Z;

        //Determinant of the second matrix.
        float dett =
            (m11 * m22 * m33) +
            (m12 * m23 * m31) +
            (m13 * m21 * m32) -
            (m11 * m23 * m32) -
            (m12 * m21 * m33) -
            (m13 * m22 * m31);

        //t values of the point of intersection.
        float s = dets / denominator;
        float t = dett / denominator;

        //The points of intersection.
        Vector3 point1 = ray1.Position + (s * ray1.Direction);
        Vector3 point2 = ray2.Position + (t * ray2.Direction);

        //If the points are not equal, no intersection has occurred.
        if (MathF.Abs(point2.X - point1.X) > MathUtil.ZeroTolerance ||
            MathF.Abs(point2.Y - point1.Y) > MathUtil.ZeroTolerance ||
            MathF.Abs(point2.Z - point1.Z) > MathUtil.ZeroTolerance)
        {
            point = Vector3.Zero;
            return false;
        }

        point = point1;
        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a <see cref="Plane"/>.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="plane">The plane to test.</param>
    /// <param name="distance">When the method completes, contains the distance of the intersection,
    /// or 0 if there was no intersection.</param>
    /// <returns>Whether the two objects intersect.</returns>
    public static bool RayIntersectsPlane(ref readonly Ray ray, ref readonly Plane plane, out float distance)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 175

        Vector3.Dot(in plane.Normal, in ray.Direction, out var direction);

        if (MathF.Abs(direction) < MathUtil.ZeroTolerance)
        {
            distance = 0f;
            return false;
        }

        Vector3.Dot(in plane.Normal, in ray.Position, out var position);
        distance = (-plane.D - position) / direction;

        if (distance < 0f)
        {
            if (distance < -MathUtil.ZeroTolerance)
            {
                distance = 0;
                return false;
            }

            distance = 0f;
        }

        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a <see cref="Plane"/>.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="plane">The plane to test</param>
    /// <param name="point">When the method completes, contains the point of intersection,
    /// or <see cref="Vector3.Zero"/> if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool RayIntersectsPlane(ref readonly Ray ray, ref readonly Plane plane, out Vector3 point)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 175

        if (!RayIntersectsPlane(in ray, in plane, out         //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 175

        float distance))
        {
            point = Vector3.Zero;
            return false;
        }

        point = ray.Position + (ray.Direction * distance);
        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a triangle.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="vertex1">The first vertex of the triangle to test.</param>
    /// <param name="vertex2">The second vertex of the triagnle to test.</param>
    /// <param name="vertex3">The third vertex of the triangle to test.</param>
    /// <param name="distance">When the method completes, contains the distance of the intersection,
    /// or 0 if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    /// <remarks>
    /// This method tests if the ray intersects either the front or back of the triangle.
    /// If the ray is parallel to the triangle's plane, no intersection is assumed to have
    /// happened. If the intersection of the ray and the triangle is behind the origin of
    /// the ray, no intersection is assumed to have happened. In both cases of assumptions,
    /// this method returns false.
    /// </remarks>
    public static bool RayIntersectsTriangle(ref readonly Ray ray, ref readonly Vector3 vertex1, ref readonly Vector3 vertex2, ref readonly Vector3 vertex3, out float distance)
    {
        //Source: Fast Minimum Storage Ray / Triangle Intersection
        //Reference: http://www.cs.virginia.edu/~gfx/Courses/2003/ImageSynthesis/papers/Acceleration/Fast%20MinimumStorage%20RayTriangle%20Intersection.pdf

        //Compute vectors along two edges of the triangle.
        Vector3 edge1, edge2;

        //Edge 1
        edge1.X = vertex2.X - vertex1.X;
        edge1.Y = vertex2.Y - vertex1.Y;
        edge1.Z = vertex2.Z - vertex1.Z;

        //Edge2
        edge2.X = vertex3.X - vertex1.X;
        edge2.Y = vertex3.Y - vertex1.Y;
        edge2.Z = vertex3.Z - vertex1.Z;

        //Cross product of ray direction and edge2 - first part of determinant.
        Vector3 directioncrossedge2;
        directioncrossedge2.X = (ray.Direction.Y * edge2.Z) - (ray.Direction.Z * edge2.Y);
        directioncrossedge2.Y = (ray.Direction.Z * edge2.X) - (ray.Direction.X * edge2.Z);
        directioncrossedge2.Z = (ray.Direction.X * edge2.Y) - (ray.Direction.Y * edge2.X);

        //Compute the determinant.
        //Dot product of edge1 and the first part of determinant.
        float determinant = (edge1.X * directioncrossedge2.X) + (edge1.Y * directioncrossedge2.Y) + (edge1.Z * directioncrossedge2.Z);

        //If the ray is parallel to the triangle plane, there is no collision.
        //This also means that we are not culling, the ray may hit both the
        //back and the front of the triangle.
        if (determinant is > -MathUtil.ZeroTolerance and < MathUtil.ZeroTolerance)
        {
            distance = 0f;
            return false;
        }

        float inversedeterminant = 1.0f / determinant;

        //Calculate the U parameter of the intersection point.
        Vector3 distanceVector;
        distanceVector.X = ray.Position.X - vertex1.X;
        distanceVector.Y = ray.Position.Y - vertex1.Y;
        distanceVector.Z = ray.Position.Z - vertex1.Z;

        float triangleU = (distanceVector.X * directioncrossedge2.X) + (distanceVector.Y * directioncrossedge2.Y) + (distanceVector.Z * directioncrossedge2.Z);
        triangleU *= inversedeterminant;

        //Make sure it is inside the triangle.
        if (triangleU is < 0f or > 1f)
        {
            distance = 0f;
            return false;
        }

        //Calculate the V parameter of the intersection point.
        Vector3 distancecrossedge1;
        distancecrossedge1.X = (distanceVector.Y * edge1.Z) - (distanceVector.Z * edge1.Y);
        distancecrossedge1.Y = (distanceVector.Z * edge1.X) - (distanceVector.X * edge1.Z);
        distancecrossedge1.Z = (distanceVector.X * edge1.Y) - (distanceVector.Y * edge1.X);

        float triangleV = (ray.Direction.X * distancecrossedge1.X) + (ray.Direction.Y * distancecrossedge1.Y) + (ray.Direction.Z * distancecrossedge1.Z);
        triangleV *= inversedeterminant;

        //Make sure it is inside the triangle.
        if (triangleV < 0f || triangleU + triangleV > 1f)
        {
            distance = 0f;
            return false;
        }

        //Compute the distance along the ray to the triangle.
        float raydistance = (edge2.X * distancecrossedge1.X) + (edge2.Y * distancecrossedge1.Y) + (edge2.Z * distancecrossedge1.Z);
        raydistance *= inversedeterminant;

        //Is the triangle behind the ray origin?
        if (raydistance < 0f)
        {
            distance = 0f;
            return false;
        }

        distance = raydistance;
        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a triangle.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="vertex1">The first vertex of the triangle to test.</param>
    /// <param name="vertex2">The second vertex of the triangle to test.</param>
    /// <param name="vertex3">The third vertex of the triangle to test.</param>
    /// <param name="point">When the method completes, contains the point of intersection,
    /// or <see cref="Vector3.Zero"/> if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool RayIntersectsTriangle(ref readonly Ray ray, ref readonly Vector3 vertex1, ref readonly Vector3 vertex2, ref readonly Vector3 vertex3, out Vector3 point)
    {
        if (!RayIntersectsTriangle(in ray, in vertex1, in vertex2, in vertex3, out float distance))
        {
            point = Vector3.Zero;
            return false;
        }

        point = ray.Position + (ray.Direction * distance);
        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a rectangle (2D).
    /// </summary>
    /// <param name="ray">The ray to test</param>
    /// <param name="rectangleWorldMatrix">The world matrix applied on the rectangle</param>
    /// <param name="rectangleSize">The size of the rectangle in 3D</param>
    /// <param name="normalAxis">The index of axis defining the normal of the rectangle in the world. This value should be 0, 1 or 2</param>
    /// <param name="intersectionPoint">The position of the intersection point in the world</param>
    /// <returns><value>true</value> if the ray and rectangle intersects.</returns>
    public static bool RayIntersectsRectangle(ref readonly Ray ray, ref readonly Matrix rectangleWorldMatrix, ref readonly Vector3 rectangleSize, int normalAxis, out Vector3 intersectionPoint)
    {
        bool intersects;

        int testAxis1;
        int testAxis2;
        switch (normalAxis)
        {
            case 0:
                testAxis1 = 1;
                testAxis2 = 2;
                break;
            case 1:
                testAxis1 = 2;
                testAxis2 = 0;
                break;
            case 2:
                testAxis1 = 0;
                testAxis2 = 1;
                break;
            default:
                throw new ArgumentOutOfRangeException(nameof(normalAxis));
        }

        var rectanglePosition = new Vector3(rectangleWorldMatrix.M41, rectangleWorldMatrix.M42, rectangleWorldMatrix.M43);

        var normalRowStart = normalAxis << 2;
        var plane = new Plane(rectanglePosition, new Vector3(rectangleWorldMatrix[normalRowStart], rectangleWorldMatrix[normalRowStart + 1], rectangleWorldMatrix[normalRowStart + 2]));

        // early exist the planes were parallels 
        if (!plane.Intersects(in ray, out intersectionPoint))
            return false;

        // the position of the intersection point with respect to the rectangle center
        var intersectionInRectangle = intersectionPoint - rectanglePosition;

        // optimization for the simple but very frequent case where the element is not rotated
        if (rectangleWorldMatrix is { M12: 0, M13: 0, M21: 0, M23: 0, M31: 0, M32: 0 })
        {
            var halfSize1 = MathF.Abs(rectangleWorldMatrix[(testAxis1 << 2) + testAxis1] * rectangleSize[testAxis1] / 2f);
            var halfSize2 = MathF.Abs(rectangleWorldMatrix[(testAxis2 << 2) + testAxis2] * rectangleSize[testAxis2] / 2f);

            return -halfSize1 <= intersectionInRectangle[testAxis1] && intersectionInRectangle[testAxis1] <= halfSize1 &&
                         -halfSize2 <= intersectionInRectangle[testAxis2] && intersectionInRectangle[testAxis2] <= halfSize2;
        }
        // general case: decompose the rectangle into two triangles and check that all angles are less than 180 degrees in at least one of the triangles.
        else
        {
            // find the most significant component of the plane normal
            var normalTestIndex = 0;
            for (int i = 1; i < 3; i++)
            {
                if (MathF.Abs(plane.Normal[i]) > MathF.Abs(plane.Normal[normalTestIndex]))
                    normalTestIndex = i;
            }
            var normalSign = MathF.Sign(plane.Normal[normalTestIndex]);

            // the base vector
            var base1 = rectangleSize[testAxis1] * new Vector3(rectangleWorldMatrix[testAxis1 << 2], rectangleWorldMatrix[(testAxis1 << 2) + 1], rectangleWorldMatrix[(testAxis1 << 2) + 2]) / 2;
            var base2 = rectangleSize[testAxis2] * new Vector3(rectangleWorldMatrix[testAxis2 << 2], rectangleWorldMatrix[(testAxis2 << 2) + 1], rectangleWorldMatrix[(testAxis2 << 2) + 2]) / 2;

            // build the first triangle and perform the test
            var v1 = -base1 - base2 - intersectionInRectangle;
            var v2 = +base1 - base2 - intersectionInRectangle;
            var v3 = +base1 + base2 - intersectionInRectangle;

            intersects = MathF.Sign(Vector3.Cross(v1, v2)[normalTestIndex]) == normalSign &&
                         MathF.Sign(Vector3.Cross(v2, v3)[normalTestIndex]) == normalSign &&
                         MathF.Sign(Vector3.Cross(v3, v1)[normalTestIndex]) == normalSign;

            // early exit on success
            if (intersects)
                return true;

            // build second triangle and perform the test
            v1 = -base1 - base2 - intersectionInRectangle;
            v2 = +base1 + base2 - intersectionInRectangle;
            v3 = -base1 + base2 - intersectionInRectangle;

            return MathF.Sign(Vector3.Cross(v1, v2)[normalTestIndex]) == normalSign &&
                         MathF.Sign(Vector3.Cross(v2, v3)[normalTestIndex]) == normalSign &&
                         MathF.Sign(Vector3.Cross(v3, v1)[normalTestIndex]) == normalSign;
        }
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a <see cref="BoundingBox"/>.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="box">The box to test.</param>
    /// <param name="distance">When the method completes, contains the distance of the intersection,
    /// or 0 if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool RayIntersectsBox(ref readonly Ray ray, ref readonly BoundingBox box, out float distance)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 179

        distance = 0f;
        float tmax = float.MaxValue;

        if (MathF.Abs(ray.Direction.X) < MathUtil.ZeroTolerance)
        {
            if (ray.Position.X < box.Minimum.X || ray.Position.X > box.Maximum.X)
            {
                distance = 0f;
                return false;
            }
        }
        else
        {
            float inverse = 1.0f / ray.Direction.X;
            float t1 = (box.Minimum.X - ray.Position.X) * inverse;
            float t2 = (box.Maximum.X - ray.Position.X) * inverse;

            if (t1 > t2)
            {
                (t2, t1) = (t1, t2);
            }

            distance = MathF.Max(t1, distance);
            tmax = MathF.Min(t2, tmax);

            if (distance > tmax)
            {
                distance = 0f;
                return false;
            }
        }

        if (MathF.Abs(ray.Direction.Y) < MathUtil.ZeroTolerance)
        {
            if (ray.Position.Y < box.Minimum.Y || ray.Position.Y > box.Maximum.Y)
            {
                distance = 0f;
                return false;
            }
        }
        else
        {
            float inverse = 1.0f / ray.Direction.Y;
            float t1 = (box.Minimum.Y - ray.Position.Y) * inverse;
            float t2 = (box.Maximum.Y - ray.Position.Y) * inverse;

            if (t1 > t2)
            {
                (t2, t1) = (t1, t2);
            }

            distance = MathF.Max(t1, distance);
            tmax = MathF.Min(t2, tmax);

            if (distance > tmax)
            {
                distance = 0f;
                return false;
            }
        }

        if (MathF.Abs(ray.Direction.Z) < MathUtil.ZeroTolerance)
        {
            if (ray.Position.Z < box.Minimum.Z || ray.Position.Z > box.Maximum.Z)
            {
                distance = 0f;
                return false;
            }
        }
        else
        {
            float inverse = 1.0f / ray.Direction.Z;
            float t1 = (box.Minimum.Z - ray.Position.Z) * inverse;
            float t2 = (box.Maximum.Z - ray.Position.Z) * inverse;

            if (t1 > t2)
            {
                (t2, t1) = (t1, t2);
            }

            distance = MathF.Max(t1, distance);
            tmax = MathF.Min(t2, tmax);

            if (distance > tmax)
            {
                distance = 0f;
                return false;
            }
        }

        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a <see cref="BoundingBox"/>.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="box">The box to test.</param>
    /// <param name="point">When the method completes, contains the point of intersection,
    /// or <see cref="Vector3.Zero"/> if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool RayIntersectsBox(ref readonly Ray ray, ref readonly BoundingBox box, out Vector3 point)
    {
        if (!RayIntersectsBox(in ray, in box, out float distance))
        {
            point = Vector3.Zero;
            return false;
        }

        point = ray.Position + (ray.Direction * distance);
        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="sphere">The sphere to test.</param>
    /// <param name="distance">When the method completes, contains the distance of the intersection,
    /// or 0 if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool RayIntersectsSphere(ref readonly Ray ray, ref readonly BoundingSphere sphere, out float distance)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 177

        Vector3.Subtract(in ray.Position, in sphere.Center, out var m);

        float b = Vector3.Dot(m, ray.Direction);
        float c = Vector3.Dot(m, m) - (sphere.Radius * sphere.Radius);

        if (c > 0f && b > 0f)
        {
            distance = 0f;
            return false;
        }

        float discriminant = (b * b) - c;

        if (discriminant < 0f)
        {
            distance = 0f;
            return false;
        }

        distance = -b - MathF.Sqrt(discriminant);

        if (distance < 0f)
            distance = 0f;

        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Ray"/> and a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="sphere">The sphere to test.</param>
    /// <param name="point">When the method completes, contains the point of intersection,
    /// or <see cref="Vector3.Zero"/> if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool RayIntersectsSphere(ref readonly Ray ray, ref readonly BoundingSphere sphere, out Vector3 point)
    {
        if (!RayIntersectsSphere(in ray, in sphere, out float distance))
        {
            point = Vector3.Zero;
            return false;
        }

        point = ray.Position + (ray.Direction * distance);
        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Plane"/> and a point.
    /// </summary>
    /// <param name="plane">The plane to test.</param>
    /// <param name="point">The point to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static PlaneIntersectionType PlaneIntersectsPoint(ref readonly Plane plane, ref readonly Vector3 point)
    {
        Vector3.Dot(in plane.Normal, in point, out var distance);
        distance += plane.D;

        if (distance > 0f)
            return PlaneIntersectionType.Front;

        if (distance < 0f)
            return PlaneIntersectionType.Back;

        return PlaneIntersectionType.Intersecting;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Plane"/> and a <see cref="Plane"/>.
    /// </summary>
    /// <param name="plane1">The first plane to test.</param>
    /// <param name="plane2">The second plane to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool PlaneIntersectsPlane(ref readonly Plane plane1, ref readonly Plane plane2)
    {
        Vector3.Cross(in plane1.Normal, in plane2.Normal, out var direction);

        //If direction is the zero vector, the planes are parallel and possibly
        //coincident. It is not an intersection. The dot product will tell us.
        Vector3.Dot(ref direction, ref direction, out var denominator);

        return MathF.Abs(denominator) >= MathUtil.ZeroTolerance;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Plane"/> and a <see cref="Plane"/>.
    /// </summary>
    /// <param name="plane1">The first plane to test.</param>
    /// <param name="plane2">The second plane to test.</param>
    /// <param name="line">When the method completes, contains the line of intersection
    /// as a <see cref="Ray"/>, or a zero ray if there was no intersection.</param>
    /// <returns>Whether the two objects intersected.</returns>
    /// <remarks>
    /// Although a ray is set to have an origin, the ray returned by this method is really
    /// a line in three dimensions which has no real origin. The ray is considered valid when
    /// both the positive direction is used and when the negative direction is used.
    /// </remarks>
    public static bool PlaneIntersectsPlane(ref readonly Plane plane1, ref readonly Plane plane2, out Ray line)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 207

        Vector3.Cross(in plane1.Normal, in plane2.Normal, out var direction);

        //If direction is the zero vector, the planes are parallel and possibly
        //coincident. It is not an intersection. The dot product will tell us.
        Vector3.Dot(ref direction, ref direction, out var denominator);

        //We assume the planes are normalized, therefore the denominator
        //only serves as a parallel and coincident check. Otherwise we need
        //to deivide the point by the denominator.
        if (MathF.Abs(denominator) < MathUtil.ZeroTolerance)
        {
            line = new Ray();
            return false;
        }

        Vector3 temp = (plane1.D * plane2.Normal) - (plane2.D * plane1.Normal);
        Vector3.Cross(ref temp, ref direction, out var point);

        line.Position = point;
        line.Direction = direction;
        line.Direction.Normalize();

        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Plane"/> and a triangle.
    /// </summary>
    /// <param name="plane">The plane to test.</param>
    /// <param name="vertex1">The first vertex of the triangle to test.</param>
    /// <param name="vertex2">The second vertex of the triagnle to test.</param>
    /// <param name="vertex3">The third vertex of the triangle to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static PlaneIntersectionType PlaneIntersectsTriangle(ref readonly Plane plane, ref readonly Vector3 vertex1, ref readonly Vector3 vertex2, ref readonly Vector3 vertex3)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 207

        PlaneIntersectionType test1 = PlaneIntersectsPoint(in plane, in vertex1);
        PlaneIntersectionType test2 = PlaneIntersectsPoint(in plane, in vertex2);
        PlaneIntersectionType test3 = PlaneIntersectsPoint(in plane, in vertex3);

        if (test1 == PlaneIntersectionType.Front && test2 == PlaneIntersectionType.Front && test3 == PlaneIntersectionType.Front)
            return PlaneIntersectionType.Front;

        if (test1 == PlaneIntersectionType.Back && test2 == PlaneIntersectionType.Back && test3 == PlaneIntersectionType.Back)
            return PlaneIntersectionType.Back;

        return PlaneIntersectionType.Intersecting;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Plane"/> and a <see cref="BoundingBox"/>.
    /// </summary>
    /// <param name="plane">The plane to test.</param>
    /// <param name="box">The box to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static PlaneIntersectionType PlaneIntersectsBox(ref readonly Plane plane, ref readonly BoundingBox box)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 161

        Vector3 min;
        Vector3 max;

        max.X = (plane.Normal.X >= 0.0f) ? box.Minimum.X : box.Maximum.X;
        max.Y = (plane.Normal.Y >= 0.0f) ? box.Minimum.Y : box.Maximum.Y;
        max.Z = (plane.Normal.Z >= 0.0f) ? box.Minimum.Z : box.Maximum.Z;
        min.X = (plane.Normal.X >= 0.0f) ? box.Maximum.X : box.Minimum.X;
        min.Y = (plane.Normal.Y >= 0.0f) ? box.Maximum.Y : box.Minimum.Y;
        min.Z = (plane.Normal.Z >= 0.0f) ? box.Maximum.Z : box.Minimum.Z;

        Vector3.Dot(in plane.Normal, ref max, out var distance);

        if (distance + plane.D > 0.0f)
            return PlaneIntersectionType.Front;

        distance = Vector3.Dot(plane.Normal, min);

        if (distance + plane.D < 0.0f)
            return PlaneIntersectionType.Back;

        return PlaneIntersectionType.Intersecting;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Plane"/> and a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="plane">The plane to test.</param>
    /// <param name="sphere">The sphere to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static PlaneIntersectionType PlaneIntersectsSphere(ref readonly Plane plane, ref readonly BoundingSphere sphere)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 160

        Vector3.Dot(in plane.Normal, in sphere.Center, out var distance);
        distance += plane.D;

        if (distance > sphere.Radius)
            return PlaneIntersectionType.Front;

        if (distance < -sphere.Radius)
            return PlaneIntersectionType.Back;

        return PlaneIntersectionType.Intersecting;
    }

    /* This implentation is wrong
    /// <summary>
    /// Determines whether there is an intersection between a <see cref="Stride.Core.Mathematics.BoundingBox"/> and a triangle.
    /// </summary>
    /// <param name="box">The box to test.</param>
    /// <param name="vertex1">The first vertex of the triangle to test.</param>
    /// <param name="vertex2">The second vertex of the triagnle to test.</param>
    /// <param name="vertex3">The third vertex of the triangle to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool BoxIntersectsTriangle(ref BoundingBox box, ref Vector3 vertex1, ref Vector3 vertex2, ref Vector3 vertex3)
    {
        if (BoxContainsPoint(ref box, ref vertex1) == ContainmentType.Contains)
            return true;

        if (BoxContainsPoint(ref box, ref vertex2) == ContainmentType.Contains)
            return true;

        if (BoxContainsPoint(ref box, ref vertex3) == ContainmentType.Contains)
            return true;

        return false;
    }
    */

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="BoundingBox"/> and a <see cref="BoundingBox"/>.
    /// </summary>
    /// <param name="box1">The first box to test.</param>
    /// <param name="box2">The second box to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool BoxIntersectsBox(ref readonly BoundingBox box1, ref readonly BoundingBox box2)
    {
        if (box1.Minimum.X > box2.Maximum.X || box2.Minimum.X > box1.Maximum.X)
            return false;

        if (box1.Minimum.Y > box2.Maximum.Y || box2.Minimum.Y > box1.Maximum.Y)
            return false;

        if (box1.Minimum.Z > box2.Maximum.Z || box2.Minimum.Z > box1.Maximum.Z)
            return false;

        return true;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="BoundingBox"/> and a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="box">The box to test.</param>
    /// <param name="sphere">The sphere to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool BoxIntersectsSphere(ref readonly BoundingBox box, ref readonly BoundingSphere sphere)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 166

        Vector3.Clamp(in sphere.Center, in box.Minimum, in box.Maximum, out var vector);
        float distance = Vector3.DistanceSquared(sphere.Center, vector);

        return distance <= sphere.Radius * sphere.Radius;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="BoundingSphere"/> and a triangle.
    /// </summary>
    /// <param name="sphere">The sphere to test.</param>
    /// <param name="vertex1">The first vertex of the triangle to test.</param>
    /// <param name="vertex2">The second vertex of the triagnle to test.</param>
    /// <param name="vertex3">The third vertex of the triangle to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool SphereIntersectsTriangle(ref readonly BoundingSphere sphere, ref readonly Vector3 vertex1, ref readonly Vector3 vertex2, ref readonly Vector3 vertex3)
    {
        //Source: Real-Time Collision Detection by Christer Ericson
        //Reference: Page 167

        ClosestPointPointTriangle(in sphere.Center, in vertex1, in vertex2, in vertex3, out var point);
        Vector3 v = point - sphere.Center;

        Vector3.Dot(ref v, ref v, out var dot);

        return dot <= sphere.Radius * sphere.Radius;
    }

    /// <summary>
    /// Determines whether there is an intersection between a <see cref="BoundingSphere"/> and a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="sphere1">First sphere to test.</param>
    /// <param name="sphere2">Second sphere to test.</param>
    /// <returns>Whether the two objects intersected.</returns>
    public static bool SphereIntersectsSphere(ref readonly BoundingSphere sphere1, ref readonly BoundingSphere sphere2)
    {
        float radiisum = sphere1.Radius + sphere2.Radius;
        return Vector3.DistanceSquared(sphere1.Center, sphere2.Center) <= radiisum * radiisum;
    }

    /// <summary>
    /// Determines whether a <see cref="BoundingBox"/> contains a point.
    /// </summary>
    /// <param name="box">The box to test.</param>
    /// <param name="point">The point to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType BoxContainsPoint(ref readonly BoundingBox box, ref readonly Vector3 point)
    {
        if (box.Minimum.X <= point.X && box.Maximum.X >= point.X &&
            box.Minimum.Y <= point.Y && box.Maximum.Y >= point.Y &&
            box.Minimum.Z <= point.Z && box.Maximum.Z >= point.Z)
        {
            return ContainmentType.Contains;
        }

        return ContainmentType.Disjoint;
    }

    /* This implentation is wrong
    /// <summary>
    /// Determines whether a <see cref="Stride.Core.Mathematics.BoundingBox"/> contains a triangle.
    /// </summary>
    /// <param name="box">The box to test.</param>
    /// <param name="vertex1">The first vertex of the triangle to test.</param>
    /// <param name="vertex2">The second vertex of the triagnle to test.</param>
    /// <param name="vertex3">The third vertex of the triangle to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType BoxContainsTriangle(ref BoundingBox box, ref Vector3 vertex1, ref Vector3 vertex2, ref Vector3 vertex3)
    {
        ContainmentType test1 = BoxContainsPoint(ref box, ref vertex1);
        ContainmentType test2 = BoxContainsPoint(ref box, ref vertex2);
        ContainmentType test3 = BoxContainsPoint(ref box, ref vertex3);

        if (test1 == ContainmentType.Contains && test2 == ContainmentType.Contains && test3 == ContainmentType.Contains)
            return ContainmentType.Contains;

        if (test1 == ContainmentType.Contains || test2 == ContainmentType.Contains || test3 == ContainmentType.Contains)
            return ContainmentType.Intersects;

        return ContainmentType.Disjoint;
    }
    */

    /// <summary>
    /// Determines whether a <see cref="BoundingBox"/> contains a <see cref="BoundingBox"/>.
    /// </summary>
    /// <param name="box1">The first box to test.</param>
    /// <param name="box2">The second box to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType BoxContainsBox(ref readonly BoundingBox box1, ref readonly BoundingBox box2)
    {
        if (box1.Maximum.X < box2.Minimum.X || box1.Minimum.X > box2.Maximum.X)
            return ContainmentType.Disjoint;

        if (box1.Maximum.Y < box2.Minimum.Y || box1.Minimum.Y > box2.Maximum.Y)
            return ContainmentType.Disjoint;

        if (box1.Maximum.Z < box2.Minimum.Z || box1.Minimum.Z > box2.Maximum.Z)
            return ContainmentType.Disjoint;

        if (box1.Minimum.X <= box2.Minimum.X && box2.Maximum.X <= box1.Maximum.X &&
            box1.Minimum.Y <= box2.Minimum.Y && box2.Maximum.Y <= box1.Maximum.Y &&
            box1.Minimum.Z <= box2.Minimum.Z && box2.Maximum.Z <= box1.Maximum.Z)
        {
            return ContainmentType.Contains;
        }

        return ContainmentType.Intersects;
    }

    /// <summary>
    /// Determines whether a <see cref="Stride.Core.Mathematics.BoundingBox"/> contains a <see cref="Stride.Core.Mathematics.BoundingSphere"/>.
    /// </summary>
    /// <param name="box">The box to test.</param>
    /// <param name="sphere">The sphere to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType BoxContainsSphere(ref readonly BoundingBox box, ref readonly BoundingSphere sphere)
    {
        Vector3.Clamp(in sphere.Center, in box.Minimum, in box.Maximum, out var vector);
        float distance = Vector3.DistanceSquared(sphere.Center, vector);

        if (distance > sphere.Radius * sphere.Radius)
            return ContainmentType.Disjoint;

        if ((box.Minimum.X + sphere.Radius <= sphere.Center.X) && (sphere.Center.X <= box.Maximum.X - sphere.Radius) && (box.Maximum.X - box.Minimum.X > sphere.Radius) &&
            (box.Minimum.Y + sphere.Radius <= sphere.Center.Y) && (sphere.Center.Y <= box.Maximum.Y - sphere.Radius) && (box.Maximum.Y - box.Minimum.Y > sphere.Radius) &&
            (box.Minimum.Z + sphere.Radius <= sphere.Center.Z) && (sphere.Center.Z <= box.Maximum.Z - sphere.Radius) && (box.Maximum.Z - box.Minimum.Z > sphere.Radius))
        {
            return ContainmentType.Contains;
        }

        return ContainmentType.Intersects;
    }

    /// <summary>
    /// Determines whether a <see cref="BoundingSphere"/> contains a point.
    /// </summary>
    /// <param name="sphere">The sphere to test.</param>
    /// <param name="point">The point to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType SphereContainsPoint(ref readonly BoundingSphere sphere, ref readonly Vector3 point)
    {
        if (Vector3.DistanceSquared(point, sphere.Center) <= sphere.Radius * sphere.Radius)
            return ContainmentType.Contains;

        return ContainmentType.Disjoint;
    }

    /// <summary>
    /// Determines whether a <see cref="BoundingSphere"/> contains a triangle.
    /// </summary>
    /// <param name="sphere">The sphere to test.</param>
    /// <param name="vertex1">The first vertex of the triangle to test.</param>
    /// <param name="vertex2">The second vertex of the triagnle to test.</param>
    /// <param name="vertex3">The third vertex of the triangle to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType SphereContainsTriangle(ref readonly BoundingSphere sphere, ref readonly Vector3 vertex1, ref readonly Vector3 vertex2, ref readonly Vector3 vertex3)
    {
        //Source: Jorgy343
        //Reference: None

        ContainmentType test1 = SphereContainsPoint(in sphere, in vertex1);
        ContainmentType test2 = SphereContainsPoint(in sphere, in vertex2);
        ContainmentType test3 = SphereContainsPoint(in sphere, in vertex3);

        if (test1 == ContainmentType.Contains && test2 == ContainmentType.Contains && test3 == ContainmentType.Contains)
            return ContainmentType.Contains;

        if (SphereIntersectsTriangle(in sphere, in vertex1, in vertex2, in vertex3))
            return ContainmentType.Intersects;

        return ContainmentType.Disjoint;
    }

    /// <summary>
    /// Determines whether a <see cref="BoundingSphere"/> contains a <see cref="BoundingBox"/>.
    /// </summary>
    /// <param name="sphere">The sphere to test.</param>
    /// <param name="box">The box to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType SphereContainsBox(ref readonly BoundingSphere sphere, ref readonly BoundingBox box)
    {
        Vector3 vector;

        if (!BoxIntersectsSphere(in box, in sphere))
            return ContainmentType.Disjoint;

        float radiussquared = sphere.Radius * sphere.Radius;
        vector.X = sphere.Center.X - box.Minimum.X;
        vector.Y = sphere.Center.Y - box.Maximum.Y;
        vector.Z = sphere.Center.Z - box.Maximum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        vector.X = sphere.Center.X - box.Maximum.X;
        vector.Y = sphere.Center.Y - box.Maximum.Y;
        vector.Z = sphere.Center.Z - box.Maximum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        vector.X = sphere.Center.X - box.Maximum.X;
        vector.Y = sphere.Center.Y - box.Minimum.Y;
        vector.Z = sphere.Center.Z - box.Maximum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        vector.X = sphere.Center.X - box.Minimum.X;
        vector.Y = sphere.Center.Y - box.Minimum.Y;
        vector.Z = sphere.Center.Z - box.Maximum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        vector.X = sphere.Center.X - box.Minimum.X;
        vector.Y = sphere.Center.Y - box.Maximum.Y;
        vector.Z = sphere.Center.Z - box.Minimum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        vector.X = sphere.Center.X - box.Maximum.X;
        vector.Y = sphere.Center.Y - box.Maximum.Y;
        vector.Z = sphere.Center.Z - box.Minimum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        vector.X = sphere.Center.X - box.Maximum.X;
        vector.Y = sphere.Center.Y - box.Minimum.Y;
        vector.Z = sphere.Center.Z - box.Minimum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        vector.X = sphere.Center.X - box.Minimum.X;
        vector.Y = sphere.Center.Y - box.Minimum.Y;
        vector.Z = sphere.Center.Z - box.Minimum.Z;

        if (vector.LengthSquared() > radiussquared)
            return ContainmentType.Intersects;

        return ContainmentType.Contains;
    }

    /// <summary>
    /// Determines whether a <see cref="BoundingSphere"/> contains a <see cref="BoundingSphere"/>.
    /// </summary>
    /// <param name="sphere1">The first sphere to test.</param>
    /// <param name="sphere2">The second sphere to test.</param>
    /// <returns>The type of containment the two objects have.</returns>
    public static ContainmentType SphereContainsSphere(ref readonly BoundingSphere sphere1, ref readonly BoundingSphere sphere2)
    {
        float distance = Vector3.Distance(sphere1.Center, sphere2.Center);

        if (sphere1.Radius + sphere2.Radius < distance)
            return ContainmentType.Disjoint;

        if (sphere1.Radius - sphere2.Radius < distance)
            return ContainmentType.Intersects;

        return ContainmentType.Contains;
    }

    /// <summary>
    /// Determines whether a <see cref="BoundingFrustum" /> intersects or contains an AABB determined by its center and extent.
    /// Faster variant specific for frustum culling.
    /// </summary>
    /// <param name="frustum">The frustum.</param>
    /// <param name="boundingBoxExt">The bounding box ext.</param>
    /// <returns><c>true</c> if XXXX, <c>false</c> otherwise.</returns>
    public static bool FrustumContainsBox(ref readonly BoundingFrustum frustum, ref readonly BoundingBoxExt boundingBoxExt)
    {
        unsafe
        {
            fixed (Plane* planeStart = &frustum.LeftPlane)
            {
                var plane = planeStart;
                for (int i = 0; i < 6; ++i)
                {
                    // Previous code:
                    if (Vector3.Dot(boundingBoxExt.Center, plane->Normal)
                        + (boundingBoxExt.Extent.X * MathF.Abs(plane->Normal.X))
                        + (boundingBoxExt.Extent.Y * MathF.Abs(plane->Normal.Y))
                        + (boundingBoxExt.Extent.Z * MathF.Abs(plane->Normal.Z))
                        <= -plane->D)
                    {
                        return false;
                    }

                    plane++;
                }
            }

            return true;
        }
        /*
                unsafe
                {
                    fixed (Plane* planeStart = &frustum.LeftPlane)
                    fixed (Vector3* pExtent = &boundingBoxExt.Extent)
                    {
                        var plane = planeStart;
                        for (int i = 0; i < 6; ++i)
                        {
                            // Previous code:
                            //if (Vector3.Dot(boundingBoxExt.Center, plane->Normal)
                            //    + boundingBoxExt.Extent.X * Math.Abs(plane->Normal.X)
                            //    + boundingBoxExt.Extent.Y * Math.Abs(plane->Normal.Y)
                            //    + boundingBoxExt.Extent.Z * Math.Abs(plane->Normal.Z)
                            //    <= -plane->D)

                            // Optimized version (only 1 dot and cheaper Math.Abs)
                            // https://fgiesen.wordpress.com/2010/10/17/view-frustum-culling/
                            // return dot3(center, plane) + dot3(extent, absPlane) <= -plane.w;
                            // or
                            // vector4 signFlip = componentwise_and(plane, 0x80000000);
                            // vector3 centerOffset = xor(extent, signFlip)
                            // dot3(center + centerOffset, plane) <= -plane.w;

                            uint val = (((uint*)&plane->Normal)[0] & 0x80000000) ^ ((uint*)pExtent)[0];
                            var dist = plane->Normal.X * ((*(float*)(&val)) + boundingBoxExt.Center.X);

                            val = (((uint*)&plane->Normal)[1] & 0x80000000) ^ ((uint*)pExtent)[1];
                            dist += plane->Normal.Y * ((*(float*)(&val)) + boundingBoxExt.Center.Y);

                            val = (((uint*)&plane->Normal)[2] & 0x80000000) ^ ((uint*)pExtent)[2];
                            dist += plane->Normal.Z * ((*(float*)(&val)) + boundingBoxExt.Center.Z);

                            if (dist <= -plane->D)
                                return false;

                            plane++;
                        }
                    }

                    return true;
                }
        */
    }

    /// <summary>
    /// Retrieves the nearest hit object starting from the position of the ray in the direction of the ray.
    /// </summary>
    /// <typeparam name="T"></typeparam>
    /// <param name="objects">The objects that get tested for a collision with the ray.</param>
    /// <param name="ray">The ray.</param>
    /// <param name="hitObject">The hit object.</param>
    /// <param name="distance">The distance from the start of the ray.</param>
    /// <param name="point">The position of the collision.</param>
    /// <returns>Whether there was a hit.</returns>
    public static bool GetNearestHit<T>(IEnumerable<T> objects, ref readonly Ray ray, out T hitObject, out float distance, out Vector3 point)
        where T : struct, IIntersectableWithRay
    {
        bool hit = false;
        distance = float.PositiveInfinity;
        hitObject = default;

        foreach (var o in objects)
        {
            if (o.Intersects(in ray, out float d) && (d < distance))
            {
                distance = d;
                hitObject = o;
                hit = true;
            }
        }

        if (hit)
            hitObject.Intersects(in ray, out point);
        else
            point = default;

        return hit;
    }
}
